#include "axis.h"
#include <string.h>
#include "planner.h"
#include "log/zlog.hh"
#include "safety/io/safeio.h"
#include "errcode.h"
#include "rtdev/servo/jkservo.h"

// 本文件的接口，均由rtdev线程调用

using namespace axis;

void Axis::rt_task_update() { do_interp(); }

// rtdev线程调用
void Axis::update_servo_cmd(double ts, double& pos)
{
    pos_planner_->calc_s(ts, pos);
    // if (!)  // 如果出错了，还会有上一周期的值
    // {
    //     servo_->set_position(pos);  //FIXME:区分通讯层的指令
    // }
    // double tor;
    // if (!tor_planner_->calc_s(ts, tor))  // 如果出错了，还会有上一周期的值
    // {
    //     servo_->set_torq(tor);  //FIXME:区分通讯层的指令
    // }
}

// 在rtdev中被调用，
void Axis::do_interp()
{
    if (!is_alive())
    {
        return;
    }
    pos_planner_->do_interp(Planner::INTERP_LINEAR, Planner::PREDICT_HOLD_VEL);
    tor_planner_->do_interp(Planner::INTERP_LINEAR, Planner::PREDICT_HOLD_VEL);
}